/** @file m_quaternion.cpp
	@brief Quaternions */

#include "math/m_quaternion.h"

using namespace G3;

//==============================================================================
// Constructors
//==============================================================================
G3::QUATERNIONF::QUATERNIONF (float X, float Y, float Z, float W) {
    x = X;
    y = Y;
    z = Z;
    w = W;
}

G3::QUATERNIONF::QUATERNIONF (VEC3F Rot) {
    x = Rot.x;
    y = Rot.y;
    z = Rot.z;
    w = 1.0f;
}

//==============================================================================
// Calculate the value of W from X, Y and Z
//==============================================================================
void G3::QUATERNIONF::CalcW () {
    float f = 1.0f - x*x - y*y - z*z;

    if (f < 0.0f) {
        w = 0.0f;
    } else {
        w = -sqrtf (f);
    }
}

//==============================================================================
// Gets the length of quaternion
//==============================================================================
float G3::QUATERNIONF::GetLength () {
    return sqrtf (x*x + y*y + z*z + w*w);
}

//==============================================================================
// Normalizes the quaternion
//==============================================================================
void G3::QUATERNIONF::Normalize () {
    float len = GetLength ();

    if (len > 0.0f) {
        x /= len;
        y /= len;
        z /= len;
        w /= len;
    }
}

//==============================================================================
// Conjugates the quaternion
//==============================================================================
void G3::QUATERNIONF::Conjugate () {
    x = -x;
    y = -y;
    z = -z;
}

//==============================================================================
// Creates a quaternion from axis and angle
//==============================================================================
void G3::QUATERNIONF::CreateFromAxisAngle (float Angle, VEC3F Axis) {
	// No axis?
	if (Axis.x == 0 && Axis.y == 0 && Axis.z == 0)
		return;
	// No angle?
	if (Angle == 0)
		return;

    VEC3F A = Axis;
    A.Normalize ();

    float HalfTheta = (G3_DEG2RAD (Angle)) / 2.0f;

    float sa = sinf (HalfTheta);

    x = A.x * sa;
    y = A.y * sa;
    z = A.z * sa;
    w = cosf (HalfTheta);
}

//==============================================================================
// Calculates axis and angle from the quaternion
// Return values in radians
//==============================================================================
void G3::QUATERNIONF::GetAxisAngle (float *Angle, VEC3F *Axis) {
    float s = sqrtf (x * x + y * y + z * z);
    Axis->x = x / s;
    Axis->y = y / s;
    Axis->z = z / s;
    *Angle = acos (w) * 2.0f;
}

//==============================================================================
// Calculates an angle from the quaternion
// Return value in radians
//==============================================================================
float G3::QUATERNIONF::GetAngle (AXIS_ID AxisID) {
	float a = 0.0f;
	VEC3F v;

	GetAxisAngle (&a, &v);

	switch (AxisID) {
		case GAXIS_X:
			return a * v.x;
		case GAXIS_Y:
			return a * v.y;
		case GAXIS_Z:
			return a * v.z;
	}
	return a;
}

//==============================================================================
// Converts quaternion into a matrix
//==============================================================================
MATRIX4x4F G3::QUATERNIONF::GetMatrix () {
    MATRIX4x4F M;
    M.m [0] = 1.0f - 2.0f * (y * y + z * z);
    M.m [1] = 2.0f * (x * y - z * w);
    M.m [2] = 2.0f * (x * z + y * w);

    M.m [4] = 2.0f * (x * y + z * w);
    M.m [5] = 1.0f - 2.0f * (x * x + z * z);
    M.m [6] = 2.0f * (z * y - x * w);

    M.m [8] = 2.0f * (x * z - y * w);
    M.m [9] = 2.0f * (y * z + x * w);
    M.m [10]= 1.0f - 2.0f * (x * x + y * y);

    return M;
}

//==============================================================================
// Multiplies quaternions
//==============================================================================
QUATERNIONF G3::QUATERNIONF::operator*(const QUATERNIONF &q) const {
    QUATERNIONF r;

    r.x = w * q.x + x * q.w + y * q.z - z * q.y;
    r.y = w * q.y - x * q.z + y * q.w + z * q.x;
    r.z = w * q.z + x * q.y - y * q.x + z * q.w;
    r.w = w * q.w - x * q.x - y * q.y - z * q.z;

    return r;
}

//==============================================================================
// Applies the quaternion to a vector
//==============================================================================
QUATERNIONF G3::QUATERNIONF::operator*(const VEC3F &v) const {
    QUATERNIONF r;

    r.x =   w * v.x  +  y * v.z  -  z * v.y;
    r.y =   w * v.y  -  x * v.z  +  z * v.x;
    r.z =   w * v.z  +  x * v.y  -  y * v.x;
    r.w = - x * v.x  -  y * v.y  -  z * v.z;

    return r;
}

//==============================================================================
// Duplicates the quaternion
//==============================================================================
QUATERNIONF& G3::QUATERNIONF::operator=(const QUATERNIONF &q) {
    x = q.x;
    y = q.y;
    z = q.z;
    w = q.w;

    return *this;
}

//==============================================================================
// Inverts the quaternion
//==============================================================================
void G3::QUATERNIONF::Invert () {
    Conjugate ();

    float norm = x*x + y*y + z*z + w*w;

    if (norm == 0.0f)
        return;

    float inv_norm = 1.0f / norm;

    x *= inv_norm;
    y *= inv_norm;
    z *= inv_norm;
    w *= inv_norm;
}

//==============================================================================
// Experimental version of CreateFromAxisAngle
//==============================================================================
QUATERNIONF G3::Euler2Quat (VEC3F Angle) {
    QUATERNIONF q;
    VEC3F pyr (G3_DEG2RAD (Angle.x) / 2.0f,
               G3_DEG2RAD (Angle.y) / 2.0f,
               G3_DEG2RAD (Angle.z) / 2.0f);

    float sinp = sinf (pyr.x); float cosp = cosf (pyr.x);
    float siny = sinf (pyr.y); float cosy = cosf (pyr.y);
    float sinr = sinf (pyr.z); float cosr = cosf ( pyr.z);

    q.x = sinr * cosp * cosy - cosr * sinp * siny;
    q.y = cosr * sinp * cosy + sinr * cosp * siny;
    q.z = cosr * cosp * siny - sinr * sinp * cosy;
    q.w = cosr * cosp * cosy + sinr * sinp * siny;

    q.Normalize ();

    return q;
}

//==============================================================================
// Conjugates a quaternion
//==============================================================================
QUATERNIONF G3::Conjugate (QUATERNIONF q) {
    return QUATERNIONF (-q.x, -q.y, -q.z, q.w);
}

//==============================================================================
// Applies a quaternion to a vector
//==============================================================================
VEC3F G3::ApplyQuat (VEC3F v, QUATERNIONF q) {
    QUATERNIONF q1, q2;
//    VEC3F V = v;
//    V.Normalize ();

    //q1 = QUATERNIONF (V.x, V.y, V.z, 0.0f);
    q1 = QUATERNIONF (v.x, v.y, v.z, 0.0f);

/*    q2 = q1 * Conjugate (q);
    q2 = q * q2;*/

    q2 = q * q1;
    q2 = q2 * Conjugate (q);

    return VEC3F (q2.x, q2.y, q2.z);
}

//==============================================================================
// Dot product of two quaternions
//==============================================================================
float G3::DotProduct (const QUATERNIONF &q1, const QUATERNIONF &q2) {
    return (q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w);
}

//==============================================================================
// Spherical linear interpolation between quaternions
//==============================================================================
QUATERNIONF G3::Slerp (const QUATERNIONF &a, const QUATERNIONF &b, float factor) {
    // Check for out-of range parameter and return edge points if so
    if (factor <= 0.0f) return a;
    if (factor >= 1.0f) return b;

    // Compute "cosine of angle between quaternions" using dot product
    float cosOmega = DotProduct (a, b);

    // If negative dot, use -q1.  Two quaternions q and -q
    // represent the same rotation, but may produce
    // different slerp.  We chose q or -q to rotate using
    // the acute angle.
    QUATERNIONF c = b;
    if (cosOmega < 0.0f) {
        c.x = -c.x;
        c.y = -c.y;
        c.z = -c.z;
        c.w = -c.w;
        cosOmega = -cosOmega;
    }

    // We should have two unit quaternions, so dot should be <= 1.0
    Assert (cosOmega < 1.1f, "ERROR! Slerp: cosOmega >= 1.1\n")

    // Compute interpolation fraction, checking for quaternions
    // almost exactly the same
    float k0, k1;

    if (cosOmega > 0.9999f) {
        k0 = 1.0f - factor;
        k1 = factor;
    } else {
        // Compute the sin of the angle using the
        // trig identity sin^2(omega) + cos^2(omega) = 1
        float sinOmega = sqrtf (1.0f - (cosOmega * cosOmega));

        // Compute the angle from its sin and cosine
        float omega = atan2f (sinOmega, cosOmega);

        // Compute inverse of denominator, so we only have
        // to divide once
        float ovrOmega = 1.0f / sinOmega;

        // Compute interpolation parameters
        k0 = sinf ((1.0f - factor) * omega) * ovrOmega;
        k1 = sinf (factor * omega) * ovrOmega;
    }

    // Interpolate and return new quaternion
    return QUATERNIONF (k0 * a.x + k1 * b.x,
                        k0 * a.y + k1 * b.y,
                        k0 * a.z + k1 * b.z,
                        k0 * a.w + k1 * b.w);
}
